#include <chrono>
#include <functional>
#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include <inttypes.h>
#include <sensor_msgs/msg/imu.hpp>

#if defined(ROS_DISTRO_FOXY)
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#elif defined(ROS_DISTRO_HUMBLE)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#error "Unsupported or Unknown ROS Distro"
#endif

#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <string>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/time.hpp>
using std::placeholders::_1;
using namespace std;
using namespace std::chrono_literals;


std::string imu_topic;

class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
  // 四元数，表示imu系下的gimbal_link的rotation
  tf2::Quaternion q_imu_gl;

  double neck_x;
  double neck_y;
  double neck_z;

  DynamicFrameBroadcaster()
  : Node("dynamic_frame_tf2_broadcaster")
  {
    // 从命令行参数中加载roll pitch yaw 并设置四元数 q_imu_gl的值
    this->declare_parameter<double>("roll", 0.0);
    this->declare_parameter<double>("pitch", 0.0);
    this->declare_parameter<double>("yaw", 0.0);
    double roll, pitch, yaw;
    // 此处的rpy是gimbal_link下的imu的rotation
    this->get_parameter("roll", roll);
    this->get_parameter("pitch", pitch);
    this->get_parameter("yaw", yaw);

    // neck_link的原点xyz，从参数获得
    this->declare_parameter<double>("neck_x", 0.0);
    this->declare_parameter<double>("neck_y", 0.0);
    this->declare_parameter<double>("neck_z", 0.0);
    this->get_parameter("neck_x", neck_x);
    this->get_parameter("neck_y", neck_y);
    this->get_parameter("neck_z", neck_z);
    // 
    // ...

    // log neck xyz
    RCLCPP_INFO(this->get_logger(), "neck_x: %f neck_y: %f neck_z: %f", neck_x, neck_y, neck_z);

    // ...
    // ros info
    RCLCPP_INFO(this->get_logger(), "roll: %f pitch: %f yaw: %f", roll, pitch, yaw); 
    tf2::Quaternion q_gl_imu;
    q_gl_imu.setRPY(roll, pitch, yaw);
    // log
    RCLCPP_INFO(this->get_logger(), "q_gl_imu: %f %f %f %f", q_gl_imu.getX(), q_gl_imu.getY(), q_gl_imu.getZ(), q_gl_imu.getW());
    // 取逆
    q_imu_gl.setX(-q_gl_imu.getX());
    q_imu_gl.setY(-q_gl_imu.getY());
    q_imu_gl.setZ(-q_gl_imu.getZ());
    q_imu_gl.setW(q_gl_imu.getW());
    q_imu_gl.normalize();
    // log
    RCLCPP_INFO(this->get_logger(), "q_imu_gl: %f %f %f %f", q_imu_gl.getX(), q_imu_gl.getY(), q_imu_gl.getZ(), q_imu_gl.getW());

    this->declare_parameter<std::string>("imu_topic","imu");
    this->get_parameter("imu_topic",  imu_topic);
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    sub_=this->create_subscription<sensor_msgs::msg::Imu>(imu_topic.c_str(), 10, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this,_1));
  }

private:
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_;
  void broadcast_timer_callback(const sensor_msgs::msg::Imu::SharedPtr imu_data)
  {
    rclcpp::Time now = this->get_clock()->now();
   
    tf2::Quaternion q;
    // imu在 odom系下的rotation：odom_R_imu
    q.setX(imu_data->orientation.x);
    q.setY(imu_data->orientation.y);
    q.setZ(imu_data->orientation.z);
    q.setW(imu_data->orientation.w);
    q.normalized();
    tf2::Quaternion q_final;
    // // 复合变换，odom_R_imu * imu_R_gimbal_link p = odom_R_gimbal_link p
    q_final = q * q_imu_gl;
    // q_final.normalize();
    // 现在，得到的是在odom系中央的gimbal系的位置
    // 实际上，有个neck偏移，所以我们把gimbal系做一些变换
    // odom->neck_link->gimbal_link
    // get the rpy

    tf2::Matrix3x3 m(q_final);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

    // 仅仅保留roll，得到neck_link
    geometry_msgs::msg::TransformStamped t_neck;
    t_neck.header.stamp = now;
    t_neck.header.frame_id = "odom";
    t_neck.child_frame_id = "neck_link";
    t_neck.transform.translation.x = 0;
    t_neck.transform.translation.y = 0;
    t_neck.transform.translation.z = 0;

    tf2::Quaternion q_neck;
    q_neck.setRPY(0.0, 0.0, yaw);
    t_neck.transform.rotation.x = q_neck.getX();
    t_neck.transform.rotation.y = q_neck.getY();
    t_neck.transform.rotation.z = q_neck.getZ();
    t_neck.transform.rotation.w = q_neck.getW();
    
    tf_broadcaster_->sendTransform(t_neck);
    
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = now;
    t.header.frame_id = "neck_link";
    t.child_frame_id = "gimbal_link";
    t.transform.translation.x = neck_x;
    t.transform.translation.y = neck_y;
    t.transform.translation.z = neck_z;

    // log
    // RCLCPP_INFO(this->get_logger(), "inside: neck_x: %f neck_y: %f neck_z: %f", t.transform.translation.x, t.transform.translation.y, t.transform.translation.z);

    // 仅仅保留pitch和yaw的四元数
    tf2::Quaternion q_pitch_yaw;
    q_pitch_yaw.setRPY(roll, pitch, 0.0);
    t.transform.rotation.x = q_pitch_yaw.getX();
    t.transform.rotation.y = q_pitch_yaw.getY();
    t.transform.rotation.z = q_pitch_yaw.getZ();
    t.transform.rotation.w = q_pitch_yaw.getW();
    
    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DynamicFrameBroadcaster>());
  rclcpp::shutdown();
  return 0;
}